#include "testHere.h"
#include "rngacml.h"
#include <math.h>
#include "normalization.h"
#define maxof2(X,Y) (((Y)>(X))?(Y):(X))
#define minof2(X,Y) (((Y)<(X))?(Y):(X))

#define MATLAB_INP_DIR "J:\\Hello\\Someshit"
#define C_INP_DIR "J:\\Hello\\Some2ndshit"

double trace2(int m, int n, double *A, int lda, double *B, int ldb);
double myTrace2(char transa,char transb, int m, int n, double *A, int lda, double *B, int ldb);
double myTrace3(char transa, int m, int n, int k, double* A, int lda, double* B, int ldb, double* C, int ldc);
void testMyTrace2();
void testMyTrace3();


int rank(double* A,int m, int n){

#define maxof2(X,Y) (((Y)>(X))?(Y):(X))
#define minof2(X,Y) (((Y)<(X))?(Y):(X))
	double *sing = (double*) malloc( sizeof(double) * maxof2(m,n));
	int rank1,i, info;
	dgesvd('N','N',m,n,A,m,sing,NULL,1,NULL,1,&info);
	rank1=0;
	for(i=0;i< minof2(m,n);i++)
		rank1 += ((sing[i]>1e-7) ? 1:0);
	free(sing);
	return rank1;
}
void dcopyMatrixTranspose(int m,int n, double* A, int lda, double *B, int ldb){
	int i,j;
	for(j=0;j<n;j++)
		for(i=0;i<m;i++)
			B[ldb*i + j]=A[j*lda + i];
	return;
}

void slepQRFactorize(char trans,int m, int n,double *a, double *q, double *r){
	int info, i;
	double *tau;
	tau = (double *) malloc( sizeof(double) * minof2(m,n) );
	if(trans=='T' || trans == 't'){
		dcopyMatrixTranspose(m,n,a,m,r,n);
		dgeqrf(n,m,r,n,tau,&info);
		memset(q,0,sizeof(double) * n * n);
		for(i=0;i<m;i++){
			if(i< minof2(m,n)){
				dcopy(n-(i+1),&r[i*n+i+1],1,&q[i*n+i+1],1);
				memset(&r[i*n+i+1],0,(n-(i+1))*sizeof(double));
			}
		}
		dorgqr(n,minof2(n,m),minof2(n,m),q,n,tau,&info);
	}else if(trans == 'N' || trans=='n'){
		dcopy(m*n,a,1,r,1);
		dgeqrf(m,n,r,m,tau,&info);
		memset(q,0,sizeof(double) * m * m);
		for(i=0;i<n;i++){
			if(i< minof2(m,n)){
				dcopy(m-(i+1),&r[i*m+i+1],1,&q[i*m+i+1],1);
				memset(&r[i*m+i+1],0,(m-(i+1))*sizeof(double));
			}
		}
		dorgqr(m,minof2(m,n),minof2(m,n),q,m,tau,&info);
	}
	free (tau);
}

// Assumes LDA=m U-m*min(m,n) VT-min(m,n)*n sing-min(m,n)
void slepSVD(int m, int n, double *A, int lda, double* sing, double *U,int ldu, double *VT, int ldvt){
	int info;
	double *B;
	B = (double *) malloc( sizeof(double) * m * n);
	dcopy(m*n,A,1,B,1);
	dgesvd('S','S',m,n,B,lda,sing,U,ldu,VT,ldvt,&info);
	free(B);
}

void testingSVD(){
	double *U, *VT, *sing, *A;
	int m=10,n=6;
	A = (double*) malloc( sizeof(double) * m * n);
	U = (double*) malloc( sizeof(double) * m * minof2(m,n));
	VT = (double*) malloc( sizeof(double) * minof2(m,n) * n);
	sing = (double*) malloc( sizeof(double) * minof2(m,n));
	acml_randu(A,m*n,0,1);
	memset(U,0,sizeof(double) * m * minof2(m,n));
	memset(VT,0,sizeof(double) * minof2(m,n) * n);
	memset(sing,0,sizeof(double) * minof2(m,n));
	slepSVD(m,n,A,m,sing,U,m,VT,minof2(m,n));
	printf("\nThe matrix U\n");
	printM(U,m,minof2(m,n));
	printf("\nThe matrix  VT\n");
	printM(VT,minof2(m,n),n);
	printf("\nThe matrix SING\n");
	printM(sing,minof2(m,n),1);
	free(A); free(U); free(VT); free(sing);

	m=6;
	n=10;
	A = (double*) malloc( sizeof(double) * m * n);
	U = (double*) malloc( sizeof(double) * m * minof2(m,n));
	VT = (double*) malloc( sizeof(double) * minof2(m,n) * n);
	sing = (double*) malloc( sizeof(double) * minof2(m,n));
	acml_randu(A,m*n,0,1);
	memset(U,0,sizeof(double) * m * minof2(m,n));
	memset(VT,0,sizeof(double) * minof2(m,n) * n);
	memset(sing,0,sizeof(double) * minof2(m,n));
	slepSVD(m,n,A,m,sing,U,m,VT,minof2(m,n));
	printf("\nThe matrix U\n");
	printM(U,m,minof2(m,n));
	printf("\nThe matrix  VT\n");
	printM(VT,minof2(m,n),n);
	printf("\nThe matrix SING\n");
	printM(sing,minof2(m,n),1);
	free(A); free(U); free(VT); free(sing);

}

int main(){
	int m=10,n=8,i,k=6,j;
	double *A, *y, *x, *y1, *x1, *oneVector, *x2,*y2, *B, *x3, *C;
	double *ltp,*tmp2,*tmp3,*tmp4;
	OPTS opt;
	double *tmp1;
	int info, *jpvt;
	double *tau;
	double *wr, *wi;

	double *q, *r;

	A = (double *)malloc( sizeof(double) * m * n);

	B = (double *)malloc( sizeof(double) * m * k);
	C = (double *)malloc( sizeof(double) * k * k);
	x = (double *)malloc( sizeof(double) * n);
	y = (double *)malloc( sizeof(double) * m);

	x1 = (double *)malloc( sizeof(double) * m);
	y1 = (double *)malloc( sizeof(double) * n);


	acml_randseed(324);
	acml_randu(A,m*n,0.0,1.0);
	acml_randu(x,n,0.0,1.0);
	printf("matrix A=\n");
	printM(A,m,n);
	printf("Vector x'=\n");
	printV(x,n);
	testingMV(A,x,m,n,y);
	printf("Vector y'=\n");
	printV(y,m);

	//testing OPTS
	initOPTS(&opt,m,n,0);

	//testing normalization
	opt.nFlag = 1;
	initNormalization(&opt,A);
	printf("Vector mu=\n");
	printM(opt.mu,1,n);
	printf("Vector nu=\n");
	printV(opt.nu,n);
	opt.nFlag = 2;
	initNormalization(&opt,A);
	printf("Vector mu=\n");
	printM(opt.mu,1,n);
	printf("Vector nu=\n");
	printM(opt.nu,m,1);


	
	acml_randu(x1,m,0.0,1.0);
	memset(x1,0,sizeof(double)* m);
	printf("matrix A'=\n");
	printMt(A,m,n);
	printf("Vector x1'=\n");
	printV(x1,m);
	testingMTV(A,x1,m,n,y1);
	printf("Vector y1'=\n");
	printV(y1,n);
	printf("Testing Max function %lf\n", maxof2(19.52,35) );
	//printf("Testing fabs function %lf\n", fabs(-19.52) );

	//printf("Testing abs function %d\n", abs(-56.52) );


	x2 = (double *)malloc( sizeof(double) * n);
	y2 = (double *)malloc( sizeof(double) * n);

	oneVector = (double *)malloc( sizeof(double) * n);

	for(i = 0; i< n; i++)
		oneVector[i] = 1.0;

	acml_randu(x2,n,0.0,1.0);
	printf("J=\n");
	printV(oneVector,n);
	dscal(n,10.0,oneVector,1);
	printf("x2=\n");
	printV(x2,n);
	printf("sigma*J=\n");
	printV(oneVector,n);
	memset(y2,0,sizeof(double)*n);
	printf("y2=\n");
	printV(y2,n);

	dsbmv('U',n,0,1.0,oneVector,1,x2,1,0.0,y2,1);
	printf("y2=diag(sigma*J)*x2\n");
	printV(y2,n);

	printf("x2=\n");
	printV(x2,n);
	oneVector[1] = 100.0;
	dtbmv('U','N','N',m,0,oneVector,1,x2,1);
	printf("x2=diag(sigma*J)*x2\n");
	printV(x2,n);
	printf("-exp(1)=%lf\n",-exp(1));
	printf("-n=%d",-n*m);

	printf("\n testing submatrix multiplication :\n");

	acml_randseed(3232);
	acml_randu(A,m*n,0.0,1.0);
	acml_randu(B,m*k,0.0,1.0);
	acml_randu(y,m,0.0,1.0);
	x3 = (double *)malloc( sizeof(double) * n);
	memset(x3,0,sizeof(double)*n);

	printf("\n matrix A,y,x3\n");
	printM(A,m,n);
	printM(y,m,1);
	printM(x3,n,1);

	// multiplies A(2:5,:)'y(2:5,1)
	dgemv('T',4,n,1.0,&A[2],m,&y[2],1,0.0,x3,1);

	printf("\n x3 after multiplication\n");
	printM(x3,n,1);
	// awesome its working.. :)

	acml_randu(B,m*k,0.0,1.0);
	acml_randu(C,k*k,0.0,1.0);
	printf("\n\n matrix B,C\n");
	printM(B,m,k);
	printf("\n\n");
	printM(C,k,k);
	dtrsm('R','U','N','N',m,k,1.0,C,k,B,m);
	printf("\n\n After multiplication B*Cinv = matrix B,C\n");
	printM(B,m,k);
	printf("\n\n");
	printM(C,k,k);
	printf("\n\n");

	printf("\n Printing B again\n");
	printM(B,m,k);
	printf("\n\n");
	jpvt = (int *)malloc( sizeof(int) * maxof2(m,k) );
	tau = (double *)malloc( sizeof(double) * maxof2(m,k) );
	dgeqp3(m,k,B,m,jpvt,tau,&info);
	printM(tau , minof2(m,k) , 1);
	printf("\n\n");
	printf("\n Printing B again\n");
	printM(B,m,k);

	printf("\n" MATLAB_INP_DIR "\\ yeah this thing works man.. :)");
	printf("\n" C_INP_DIR "\\ yeah this thing works man.. :)");

	printf("\n rank finding now\n");
	printf("\n The matrix B is \n");
	printM(B,m,k);
	printf("\n The rank of the matrix is %d\n",rank(B,m,k));
	printf("\n The matrix B again is \n");
	printM(B,m,k);

	free(B);
	free(jpvt);
	B = (double *) malloc( sizeof(double) * m*m);
	jpvt = (int *)malloc( sizeof(int) * m);
	tau = (double*)malloc( sizeof(double) * m);
	memset(B,0, sizeof(double) * m * m );
	memset(jpvt, 0 , sizeof(int) * m);
	memset(tau, 0 , sizeof(double) * m);
	for(i=0;i<m;i++)
		B[i*m + i] = 1;
	printf("\n The matrix B is \n");
	printM(B,m,m);

	jpvt[0] = 1;
	jpvt[1] = m/2;
	jpvt[2] = m/2 + 1;
	jpvt[7] = -m/2;

	printf("\n jpvt is \n");
	for(i=0;i<m;i++)
		printf("%d\n",jpvt[i]);
	printf("\n tau is \n");
	printM(tau,m,1);

	dgeqp3(m,m,B,m,jpvt,tau,&info);


	printf("\n The matrix B is \n");
	printM(B,m,m);

	printf("\n jpvt is \n");
	for(i=0;i<m;i++)
		printf("%d\n",jpvt[i]);
	printf("\n tau is \n");
	printM(tau,m,1);

	free(B);
	printf("\nm=%d, n=%d\n",m,n);
	B = (double *) malloc( sizeof(double) * m*n);
	acml_randu(B,m*n,0,1);
	printf("\n The matrix B is \n");
	printM(B,m,n);
	q = (double *) malloc( sizeof(double) * m*m);
	r = (double *) malloc( sizeof(double) * m*n);
	slepQRFactorize('N',m,n,B,q,r);
	printf("\n The matrix B is \n");
	printM(B,m,n);
	printf("\n The matrix Q is \n");
	printM(q,m,m);
	printf("\n The matrix R is \n");
	printM(r,m,n);

	free(q);
	free(r);
	free(B);
	m = m - n;
	n = m + n;
	m = n - m;
	printf("\nm=%d, n=%d\n",m,n);
	B = (double *) malloc( sizeof(double) * m*n);
	acml_randu(B,m*n,0,1);
	printf("\n The matrix B is \n");
	printM(B,m,n);
	q = (double *) malloc( sizeof(double) * m*m);
	r = (double *) malloc( sizeof(double) * m*n);
	slepQRFactorize('N',m,n,B,q,r);
	printf("\n The matrix B is \n");
	printM(B,m,n);
	printf("\n The matrix Q is \n");
	printM(q,m,m);
	printf("\n The matrix R is \n");
	printM(r,m,n);

	//-------
	free(q);
	free(r);
	free(B);
	printf("\nm=%d, n=%d and Transpose\n",m,n);
	B = (double *) malloc( sizeof(double) * m*n);
	acml_randu(B,m*n,0,1);
	printf("\n The matrix B is \n");
	printM(B,m,n);
	q = (double *) malloc( sizeof(double) * n*n);
	r = (double *) malloc( sizeof(double) * n*m);
	slepQRFactorize('T',m,n,B,q,r);
	printf("\n The matrix B is \n");
	printM(B,m,n);
	printf("\n The matrix Q is \n");
	printM(q,n,n);
	printf("\n The matrix R is \n");
	printM(r,n,m);

	free(q);
	free(r);
	free(B);
	m = m - n;
	n = m + n;
	m = n - m;
	printf("\nm=%d, n=%d and Transpose\n",m,n);
	B = (double *) malloc( sizeof(double) * m*n);
	acml_randu(B,m*n,0,1);
	printf("\n The matrix B is \n");
	printM(B,m,n);
	q = (double *) malloc( sizeof(double) * n*n);
	r = (double *) malloc( sizeof(double) * n*m);
	slepQRFactorize('T',m,n,B,q,r);
	printf("\n The matrix B is \n");
	printM(B,m,n);
	printf("\n The matrix Q is \n");
	printM(q,n,n);
	printf("\n The matrix R is \n");
	printM(r,n,m);

	n=6;
	free(B);
	wr = (double *) malloc( sizeof(double) * n);
	wi = (double *) malloc( sizeof(double) * n);
	B = (double *) malloc( sizeof(double) * n*n);
	acml_randu(B,n*n,0,1);
	acml_randu(B,n*n,0,1);
	acml_randu(B,n*n,-1,1);
	memset(wr,0,sizeof(double) * n);
	memset(wi,0,sizeof(double) * n);
	for(i=0;i<n;i++)
		for(j=i;j<n;j++)
			B[i*n+j] = B[j*n + i];

	printf("\n The matrix B is \n");
	printM(B,n,n);

	dgeev('N','N',n,B,n,wr,wi,NULL,1,NULL,1,&info);
	printf("\n The matrix B is \n");
	printM(B,n,n);
	printf("\n The Real eigen Values are \n");
	printM(wr,1,n);
	printf("\n The Imaginary eigen Values are \n");
	printM(wi,1,n);

	free(B);
	free(A);
	B = (double*) malloc( sizeof(double) * 7*4);
	A = (double*) malloc( sizeof(double) * 7*4);
	acml_randu(B,7*4,0,1);
	printf("\n The matrix B is \n");
	printM(B,7,4);
	dcopyMatrixTranspose(7,4,B,7,A,4);
	printf("\n The matrix B is \n");
	printM(B,7,4);
	printf("\n The matrix A is \n");
	printM(A,4,7);

	testingSVD();

	free(B);
	free(A);
	A = (double*) malloc( sizeof(double) * 5*8);
	B = (double*) malloc( sizeof(double) * 3*6);
	printf("\Testing Interchange \n");
	acml_randu(A,5*8,0,1);
	acml_randu(B,3*6,0,1);
	printf("\n The matrix A is \n");
	printM(A,5,8);
	printf("\n The matrix B is \n");
	printM(B,3,6);
	printf("\Interchanging...\n");
	A = (long)A ^ (long)B ;
	B = (long)B ^ (long)A ;
	A = (long)A ^ (long)B ;

	printf("\n The matrix A is \n");
	printM(A,3,6);
	printf("\n The matrix B is \n");
	printM(B,5,8);

	printf("\nsizeof(double *) is %d, and that of long is %d, and of int is %d\n", sizeof(double*), sizeof(long), sizeof(int));

	testMyTrace2();
	testMyTrace3();
	scanf("%d",&info);
	return 0;
}